#ifndef DEPTH_IMG
#define DEPTH_IMG 
 
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <tf/transform_broadcaster.h>
#include <mutex>
#include"utils.hpp"
#include"calib.hpp"
using namespace std;
using PointType = pcl::PointXYZI;


class RgbDepthHandle{
    public:
        int img_h_ = 480;
        int img_w_ = 640;

        cv::Mat depth_img_, rgb_img_resize_, rgb_img_show_, lidar_img_show_, rgb_img_, depth_img_resize_;
        cv::Mat mask_, depth_resize_,save_img_;

        Calib calib_;
        PointType object_center_;
        bool is_get_object_pc_ =false;

        ros::Time time_stamp_;
    
    private:
        mutex img_lock_;
        int scale_w_,scale_h_;
        float scale_;
        bool is_cal_scale_ = false;
        vector <cv :: Point > rgb_conners_, mask_conners_;
        cv::Mat pc_idx_img_;
        string obj_inf_show_;
        string save_dir_;
        float p2_[12] = {610.88   ,    0.     ,  319.16538,    0.     ,
            0.     ,  611.23944,  250.63283,    0.     ,
            0.     ,    0.     ,    1.     ,    0.};
        string pack_path_;
        
    public:
        RgbDepthHandle(){ 
            time_t t = time(NULL);
            char ch[64] = {0};
            char result[100] = {0};
            strftime(ch, sizeof(ch) - 1, "%Y%m%d--%H%M%S", localtime(&t));
            sprintf(result, "%s", ch);
            pack_path_ = ros::package::getPath("mm_perceive");
            string parent_dir = pack_path_ + "/data/rs_img/";
            string time_name = string(result);
            string bar = "/";
            save_dir_ = parent_dir+time_name+bar;
            mkdir(save_dir_.c_str(),S_IRUSR | S_IWUSR | S_IXUSR | S_IRWXG | S_IRWXO);
         }
          
        ~RgbDepthHandle() {
        }   

        cv::Mat get_img_show(){
            lock_guard<mutex> img_lock(img_lock_);
            cv::Mat img_show_temp;
            rgb_img_show_.copyTo(img_show_temp);
            return img_show_temp;
        }

        bool get_is_get_obj(){
            lock_guard<mutex> img_lock(img_lock_);
            return is_get_object_pc_;
        }

        void get_img_extrinsics(cv::Mat rgb_img, cv::Mat depth_img, float* T_rs2base, ros::Time time_stamp){
            lock_guard<mutex> img_lock(img_lock_);
            rgb_img_ = rgb_img;
            depth_img_ = depth_img;
            time_stamp_ = time_stamp;
            if(!is_cal_scale_){
                scale_ = img_w_ / rgb_img.size().width;
                scale_w_ =  static_cast<int>(rgb_img.size().width * scale_);
                scale_h_ =  static_cast<int>(rgb_img.size().height * scale_);

                //read file p2
                ifstream read_p2,read_t_lidar2camera;
                read_p2.open(pack_path_+"/camera_param/rs_hand_intrinsics.txt");
                string s_line, para1,para2,para3,para4;
                for(int count_i = 0; count_i<3;count_i++){
                    getline(read_p2,s_line);
                    istringstream s_in(s_line);
                    s_in>>para1>>para2>>para3>>para4;
                    p2_[count_i*4] = atof(para1.c_str());
                    p2_[count_i*4 + 1] = atof(para2.c_str());
                    p2_[count_i*4 + 2] = atof(para3.c_str());
                    p2_[count_i*4 + 3] = atof(para4.c_str());
                }

                calib_.get_img_size_extrinsics(rgb_img.size().width, rgb_img.size().height , p2_, T_rs2base);
                is_cal_scale_ = true;
            }
            resize(rgb_img_, rgb_img_resize_, cv::Size(scale_w_, scale_h_));
            resize(depth_img_, depth_resize_, cv::Size(scale_w_, scale_h_));
        }

        void roi_point_init(){
            obj_inf_show_  = string();
            clear_vector(rgb_conners_);
            clear_vector(mask_conners_);
        }

        void record_point(int x, int y) {
            lock_guard<mutex> img_lock(img_lock_);
            rgb_conners_.push_back(cv::Point(x,y));
            mask_conners_.push_back(cv::Point(int(x / scale_), int(y / scale_)));
            is_get_object_pc_ =false;
        }

        void draw_contours(string kb_inf){
            lock_guard<mutex> img_lock(img_lock_);
            rgb_img_resize_.copyTo(rgb_img_show_);
            
            for(int y = 0; y<rgb_img_show_.size().height; y++){
                for(int x = 0; x<rgb_img_show_.size().width; x++){
                    if(depth_resize_.at<uint16_t>(y,x) == 0  ||  depth_resize_.at<uint16_t>(y,x)*0.001 >= 5){
                        rgb_img_show_.at<cv::Vec3b>(y, x)[0] = 0;
                        rgb_img_show_.at<cv::Vec3b>(y, x)[1] = 0;
                        rgb_img_show_.at<cv::Vec3b>(y, x)[2] = 0;
                    }
                }
            }
            switch(rgb_conners_.size()){
                case 0:{
                    break;
                }
                case 1 :{
                    circle(rgb_img_show_, rgb_conners_[0], 3, cv::Scalar(0,0, 255), 2);
                    break;
                }
                case 2 :{
                    line(rgb_img_show_, rgb_conners_[0], rgb_conners_[1], cv::Scalar(0,0, 255), 2);
                    break;
                }
                default :{
                    vector < vector <cv :: Point >>  contours;
                    contours.push_back(rgb_conners_);
                    cv::drawContours(rgb_img_show_, contours,0, cv::Scalar(0, 0, 255),2);   
                    if(!kb_inf.empty()){
                        obj_inf_show_ = obj_inf_show_ + kb_inf;
                    }
                    cv::putText(rgb_img_show_, obj_inf_show_, cv::Point(0,25),cv::FONT_HERSHEY_SIMPLEX,1,cv::Scalar(0,0,255),2,8,false);
                    break;
                }
            }
            if(  kb_inf == string("reset")){  //need more time this can get the string
                roi_point_init();
                return;
            }
        }

        ObjectPc get_object_points_rs(){
            lock_guard<mutex> img_lock(img_lock_);
            if(mask_conners_.size()<3){
                roi_point_init();
                pcl::PointCloud<PointType>::Ptr p_temp(new pcl::PointCloud<PointType>);
                ObjectPc temp;
                temp.p_object_pc = p_temp;
                return temp;
            }
            mask_ = cv::Mat(cv::Size(rgb_img_.size().width, rgb_img_.size().height), CV_8UC1,cv::Scalar(0));
            vector < vector <cv :: Point >>  contours;
            contours.push_back(mask_conners_);
            cv::drawContours(mask_, contours,0, (255),CV_FILLED);   

            pcl::PointCloud<PointType>::Ptr  object_pc(new pcl::PointCloud<PointType>);
            PointType point;
            Eigen::Vector4f e_p;
            for(int y = 0; y<mask_.size().height; y++){
                for(int x = 0; x<mask_.size().width; x++){
                    if(mask_.at<uchar>(y,x) > 0 && depth_img_.at<uint16_t>(y,x) > 0 && depth_img_.at<uint16_t>(y,x)*0.001 < 5){
                        e_p = calib_.img2lidar(x, y, 0.001 * depth_img_.at<uint16_t>(y,x));
                        point.x = e_p(0);point.y = e_p(1);point.z = e_p(2);
                        object_pc->points.push_back(point);
                    }
                }
            }

            if(object_pc->empty()){
                roi_point_init();
                pcl::PointCloud<PointType>::Ptr p_temp(new pcl::PointCloud<PointType>);
                ObjectPc temp;
                temp.p_object_pc = p_temp;
                is_get_object_pc_ = false;
                return temp;
            }

            ObjectPc data_ret;
            data_ret.idx = obj_inf_show_;
            data_ret.time_stamp = time_stamp_;
            data_ret.p_object_pc = object_pc;
            //save img
            save_img(obj_inf_show_);
            //save img
            is_get_object_pc_ = true;
            roi_point_init();
            return data_ret;
        }

        void save_img(string idx){
            if(rgb_conners_.size()>2){
                rgb_img_resize_.copyTo(save_img_);
                vector < vector <cv :: Point >>  contours;
                contours.push_back(rgb_conners_);
                cv::drawContours(save_img_, contours,0, cv::Scalar(0, 0, 255),2);   
                cv::imwrite(save_dir_+idx+string("_a.jpg"),save_img_);
                cv::imwrite(save_dir_+idx+string("_b.jpg"),save_img_);
            }else{
                cout<<"ERROR   img bbox less 3 points"<<endl;
            }
        }
};

// class PcDataLoader{
//     public:
        
// }
#endif
